{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Tutorial: Interactive Scenario Simulation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This tutorial explains how run interactive scenarios using the code from the repository [https://gitlab.lrz.de/tum-cps/commonroad-interactive-scenarios](https://gitlab.lrz.de/tum-cps/commonroad-interactive-scenarios). We obtain interactive scenarios by coupling CommonRoad with the traffic simulator [SUMO](https://sumo.dlr.de/docs/index.html). Below, a scenario is referred to as **recorded** if all other traffic participants therein follow pre-recorded trajectories, whereas in an **interactive** scenario, other traffic participants react to the ego vehicle. Currently supported options for interactive scenario simulation are:\n",
    "  1. Simulation without an ego vehicle.\n",
    "  2. Simulation with a plugged-in motion planner.\n",
    "  3. Simulation with a solution trajectory to the planning problem.\n",
    "\n",
    "In the following sections, you will be able to observe the varied behaviors of other traffic participants towards the motion of the ego vehicle. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 0. Preparation\n",
    "Before you proceed with this tutorial, we assume that:\n",
    "* you have gone through the tutorials on [CommonRoad Input-Output](https://commonroad.in.tum.de/tutorials/commonroad-interface) and have a basic understanding of the CommonRoad scenarios.\n",
    "* you have successfully installed all necessary modules for the interactive benchmark by running the installation script.\n",
    "\n",
    "Here, we import necessary packages for the simulation. Also, we specify some required path and parameters required for the simulation. As an example, we demonstrate the simulation with scenario `DEU_Frankfurt-34_11_I-1`, which is located under the folder `./interactive_scenarios`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "\n",
    "import os, sys\n",
    "sys.path.append(os.path.join(os.getcwd(), \"../\"))\n",
    "\n",
    "from simulation.simulations import simulate_without_ego, simulate_with_solution, simulate_with_planner\n",
    "from simulation.utility import visualize_scenario_with_trajectory, save_solution\n",
    "from commonroad.common.file_writer import CommonRoadFileWriter, OverwriteExistingFile\n",
    "from commonroad.common.solution import CommonRoadSolutionReader, VehicleType, VehicleModel, CostFunction\n",
    "from commonroad.scenario.scenario import Tag\n",
    "\n",
    "path = os.path.abspath(\"\")\n",
    "\n",
    "# specify required arguments\n",
    "name_scenario = \"DEU_Cologne-63_5_I-1\"\n",
    "\n",
    "# replace with local folder path (in this case we cloned the whole repository from https://gitlab.lrz.de/tum-cps/commonroad-scenarios/-/tree/2020a_scenarios):\n",
    "folder_scenarios = os.path.join(path, \"../scenarios/tutorial/\")\n",
    "path_scenario = os.path.join(folder_scenarios, name_scenario)\n",
    "# path where solutions are stored\n",
    "path_solutions = os.path.join(path, \"../outputs/solutions\")\n",
    "\n",
    "# path to store output video\n",
    "path_video = os.path.join(path, \"../outputs/videos\")\n",
    "\n",
    "# path to store simulated scenarios\n",
    "path_scenarios_simulated = os.path.join(path, \"../outputs/simulated_scenarios\")\n",
    "\n",
    "# demo attributes for saving the simulated scenarios\n",
    "author = 'Max Mustermann'\n",
    "affiliation = 'Technical University of Munich, Germany'\n",
    "source = ''\n",
    "tags = {Tag.URBAN}\n",
    "\n",
    "vehicle_type = VehicleType.FORD_ESCORT\n",
    "vehicle_model = VehicleModel.KS\n",
    "cost_function = CostFunction.TR1"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. Simulation without an Ego Vehicle\n",
    "\n",
    "As the start, we demonstrate how to run the simulation in SUMO without considering the ego vehicle. This is beneficial for observing the default behaviour of other traffic participants. The simulation is executed by calling the function **simulate_without_ego()** with the following arguments:\n",
    "* **interactive_scenario_path**: path to the folder containing required SUMO network files for the sceanrio. \n",
    "* **output_folder_path**: path to the output folder.\n",
    "* **create_video**: indicates whether to create a video of the simulated scenario. **Default value**: False\n",
    "\n",
    "We save the simulated scenario to a local CommonRoad xml file.\n",
    "\n",
    "Note: it is alright if you see some SUMO simulation-related warnings."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "ename": "AssertionError",
     "evalue": "<Obstacle/initial_state>: argument initial_state of wrong type. Expected types: <class 'commonroad.scenario.state.InitialState'>. Got type: <class 'commonroad.scenario.state.CustomState'>.",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mAssertionError\u001b[0m                            Traceback (most recent call last)",
      "\u001b[1;32m/home/string/Programs/ad_benchmark_learn/side_package/commonroad-interactive-scenarios/tutorials/1_tutorial_scenario_simulation.ipynb 单元格 6\u001b[0m line \u001b[0;36m2\n\u001b[1;32m      <a href='vscode-notebook-cell:/home/string/Programs/ad_benchmark_learn/side_package/commonroad-interactive-scenarios/tutorials/1_tutorial_scenario_simulation.ipynb#W5sZmlsZQ%3D%3D?line=0'>1</a>\u001b[0m \u001b[39m# run simulation, a video animation of the simulation is stored in the end\u001b[39;00m\n\u001b[0;32m----> <a href='vscode-notebook-cell:/home/string/Programs/ad_benchmark_learn/side_package/commonroad-interactive-scenarios/tutorials/1_tutorial_scenario_simulation.ipynb#W5sZmlsZQ%3D%3D?line=1'>2</a>\u001b[0m scenario_without_ego, pps \u001b[39m=\u001b[39m simulate_without_ego(interactive_scenario_path\u001b[39m=\u001b[39;49mpath_scenario,\n\u001b[1;32m      <a href='vscode-notebook-cell:/home/string/Programs/ad_benchmark_learn/side_package/commonroad-interactive-scenarios/tutorials/1_tutorial_scenario_simulation.ipynb#W5sZmlsZQ%3D%3D?line=2'>3</a>\u001b[0m                                                  output_folder_path\u001b[39m=\u001b[39;49mpath_video,\n\u001b[1;32m      <a href='vscode-notebook-cell:/home/string/Programs/ad_benchmark_learn/side_package/commonroad-interactive-scenarios/tutorials/1_tutorial_scenario_simulation.ipynb#W5sZmlsZQ%3D%3D?line=3'>4</a>\u001b[0m                                                  create_video\u001b[39m=\u001b[39;49m\u001b[39mTrue\u001b[39;49;00m)\n\u001b[1;32m      <a href='vscode-notebook-cell:/home/string/Programs/ad_benchmark_learn/side_package/commonroad-interactive-scenarios/tutorials/1_tutorial_scenario_simulation.ipynb#W5sZmlsZQ%3D%3D?line=4'>5</a>\u001b[0m \u001b[39m# write simulated scenario to CommonRoad xml file\u001b[39;00m\n\u001b[1;32m      <a href='vscode-notebook-cell:/home/string/Programs/ad_benchmark_learn/side_package/commonroad-interactive-scenarios/tutorials/1_tutorial_scenario_simulation.ipynb#W5sZmlsZQ%3D%3D?line=5'>6</a>\u001b[0m fw \u001b[39m=\u001b[39m CommonRoadFileWriter(scenario_without_ego, pps, author, affiliation, source, tags)\n",
      "File \u001b[0;32m~/Programs/ad_benchmark_learn/side_package/commonroad-interactive-scenarios/tutorials/../simulation/simulations.py:196\u001b[0m, in \u001b[0;36msimulate_without_ego\u001b[0;34m(interactive_scenario_path, output_folder_path, create_video, use_sumo_manager, num_of_steps)\u001b[0m\n\u001b[1;32m    194\u001b[0m num_of_steps \u001b[39m=\u001b[39m conf\u001b[39m.\u001b[39msimulation_steps \u001b[39mif\u001b[39;00m num_of_steps \u001b[39mis\u001b[39;00m \u001b[39mNone\u001b[39;00m \u001b[39melse\u001b[39;00m num_of_steps\n\u001b[1;32m    195\u001b[0m \u001b[39m# simulation without ego vehicle\u001b[39;00m\n\u001b[0;32m--> 196\u001b[0m simulated_scenario_without_ego, _ \u001b[39m=\u001b[39m simulate_scenario(SimulationOption\u001b[39m.\u001b[39;49mWITHOUT_EGO, conf,\n\u001b[1;32m    197\u001b[0m                                                       scenario_wrapper,\n\u001b[1;32m    198\u001b[0m                                                       interactive_scenario_path,\n\u001b[1;32m    199\u001b[0m                                                       num_of_steps\u001b[39m=\u001b[39;49mnum_of_steps,\n\u001b[1;32m    200\u001b[0m                                                       planning_problem_set\u001b[39m=\u001b[39;49mplanning_problem_set,\n\u001b[1;32m    201\u001b[0m                                                       solution\u001b[39m=\u001b[39;49m\u001b[39mNone\u001b[39;49;00m,\n\u001b[1;32m    202\u001b[0m                                                       use_sumo_manager\u001b[39m=\u001b[39;49muse_sumo_manager)\n\u001b[1;32m    203\u001b[0m simulated_scenario_without_ego\u001b[39m.\u001b[39mscenario_id \u001b[39m=\u001b[39m scenario\u001b[39m.\u001b[39mscenario_id\n\u001b[1;32m    205\u001b[0m \u001b[39mif\u001b[39;00m create_video:\n",
      "File \u001b[0;32m~/Programs/ad_benchmark_learn/side_package/commonroad-interactive-scenarios/tutorials/../simulation/simulations.py:158\u001b[0m, in \u001b[0;36msimulate_scenario\u001b[0;34m(mode, conf, scenario_wrapper, scenario_path, num_of_steps, planning_problem_set, solution, use_sumo_manager)\u001b[0m\n\u001b[1;32m    155\u001b[0m     run_simulation()\n\u001b[1;32m    157\u001b[0m \u001b[39m# retrieve the simulated scenario in CR format\u001b[39;00m\n\u001b[0;32m--> 158\u001b[0m simulated_scenario \u001b[39m=\u001b[39m sumo_sim\u001b[39m.\u001b[39;49mcommonroad_scenarios_all_time_steps()\n\u001b[1;32m    160\u001b[0m \u001b[39m# stop the simulation\u001b[39;00m\n\u001b[1;32m    161\u001b[0m sumo_sim\u001b[39m.\u001b[39mstop()\n",
      "File \u001b[0;32m~/Programs/ad_benchmark_learn/.venv/lib/python3.10/site-packages/sumocr/interface/sumo_simulation.py:346\u001b[0m, in \u001b[0;36mSumoSimulation.commonroad_scenarios_all_time_steps\u001b[0;34m(self)\u001b[0m\n\u001b[1;32m    343\u001b[0m     \u001b[39mfor\u001b[39;00m lanelet \u001b[39min\u001b[39;00m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mcr_scenario\u001b[39m.\u001b[39mlanelet_network\u001b[39m.\u001b[39mlanelets:\n\u001b[1;32m    344\u001b[0m         lanelet\u001b[39m.\u001b[39mdynamic_obstacles_on_lanelet \u001b[39m=\u001b[39m {}\n\u001b[0;32m--> 346\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mcr_scenario\u001b[39m.\u001b[39madd_objects(\u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49m_get_cr_obstacles_all())\n\u001b[1;32m    347\u001b[0m \u001b[39mreturn\u001b[39;00m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mcr_scenario\n",
      "File \u001b[0;32m~/Programs/ad_benchmark_learn/.venv/lib/python3.10/site-packages/sumocr/interface/sumo_simulation.py:755\u001b[0m, in \u001b[0;36mSumoSimulation._get_cr_obstacles_all\u001b[0;34m(self)\u001b[0m\n\u001b[1;32m    748\u001b[0m         shape_lanelets \u001b[39m=\u001b[39m {lanelet_id \u001b[39mfor\u001b[39;00m lanelet_ids \u001b[39min\u001b[39;00m\n\u001b[1;32m    749\u001b[0m                           \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mcr_scenario\u001b[39m.\u001b[39mlanelet_network\u001b[39m.\u001b[39mfind_lanelet_by_position(\n\u001b[1;32m    750\u001b[0m                               [initial_state\u001b[39m.\u001b[39mposition \u001b[39m+\u001b[39m v\n\u001b[1;32m    751\u001b[0m                                \u001b[39mfor\u001b[39;00m v \u001b[39min\u001b[39;00m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mobstacle_shapes[veh_id]\u001b[39m.\u001b[39mvertices])\n\u001b[1;32m    752\u001b[0m                           \u001b[39mfor\u001b[39;00m lanelet_id \u001b[39min\u001b[39;00m lanelet_ids}\n\u001b[1;32m    754\u001b[0m     signal_states \u001b[39m=\u001b[39m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39msignal_states[veh_id]\n\u001b[0;32m--> 755\u001b[0m     dynamic_obstacle \u001b[39m=\u001b[39m DynamicObstacle(\n\u001b[1;32m    756\u001b[0m         obstacle_id\u001b[39m=\u001b[39;49mveh_id,\n\u001b[1;32m    757\u001b[0m         obstacle_type\u001b[39m=\u001b[39;49mobstacle_type,\n\u001b[1;32m    758\u001b[0m         initial_state\u001b[39m=\u001b[39;49minitial_state,\n\u001b[1;32m    759\u001b[0m         obstacle_shape\u001b[39m=\u001b[39;49mobstacle_shape,\n\u001b[1;32m    760\u001b[0m         prediction\u001b[39m=\u001b[39;49mobstacle_prediction,\n\u001b[1;32m    761\u001b[0m         initial_center_lanelet_ids\u001b[39m=\u001b[39;49mcenter_lanelets \u001b[39mif\u001b[39;49;00m center_lanelets \u001b[39melse\u001b[39;49;00m \u001b[39mNone\u001b[39;49;00m,\n\u001b[1;32m    762\u001b[0m         initial_shape_lanelet_ids\u001b[39m=\u001b[39;49mshape_lanelets \u001b[39mif\u001b[39;49;00m shape_lanelets \u001b[39melse\u001b[39;49;00m \u001b[39mNone\u001b[39;49;00m,\n\u001b[1;32m    763\u001b[0m         initial_signal_state\u001b[39m=\u001b[39;49msignal_states[\u001b[39m0\u001b[39;49m] \u001b[39mif\u001b[39;49;00m signal_states \u001b[39melse\u001b[39;49;00m \u001b[39mNone\u001b[39;49;00m,\n\u001b[1;32m    764\u001b[0m         signal_series\u001b[39m=\u001b[39;49msignal_states[\u001b[39m1\u001b[39;49m:] \u001b[39mif\u001b[39;49;00m signal_states \u001b[39melse\u001b[39;49;00m \u001b[39mNone\u001b[39;49;00m)  \u001b[39m# add a trajectory element\u001b[39;00m\n\u001b[1;32m    765\u001b[0m     obstacles\u001b[39m.\u001b[39mappend(dynamic_obstacle)\n\u001b[1;32m    766\u001b[0m \u001b[39melse\u001b[39;00m:\n",
      "File \u001b[0;32m~/Programs/ad_benchmark_learn/.venv/lib/python3.10/site-packages/commonroad/scenario/obstacle.py:402\u001b[0m, in \u001b[0;36mDynamicObstacle.__init__\u001b[0;34m(self, obstacle_id, obstacle_type, obstacle_shape, initial_state, prediction, initial_center_lanelet_ids, initial_shape_lanelet_ids, initial_signal_state, signal_series, initial_meta_information_state, meta_information_series, external_dataset_id, history, signal_history, center_lanelet_ids_history, shape_lanelet_ids_history, **kwargs)\u001b[0m\n\u001b[1;32m    400\u001b[0m \u001b[39mfor\u001b[39;00m (field, value) \u001b[39min\u001b[39;00m kwargs\u001b[39m.\u001b[39mitems():\n\u001b[1;32m    401\u001b[0m     \u001b[39msetattr\u001b[39m(\u001b[39mself\u001b[39m, field, value)\n\u001b[0;32m--> 402\u001b[0m Obstacle\u001b[39m.\u001b[39;49m\u001b[39m__init__\u001b[39;49m(\u001b[39mself\u001b[39;49m, obstacle_id\u001b[39m=\u001b[39;49mobstacle_id, obstacle_role\u001b[39m=\u001b[39;49mObstacleRole\u001b[39m.\u001b[39;49mDYNAMIC,\n\u001b[1;32m    403\u001b[0m                   obstacle_type\u001b[39m=\u001b[39;49mobstacle_type, obstacle_shape\u001b[39m=\u001b[39;49mobstacle_shape, initial_state\u001b[39m=\u001b[39;49minitial_state,\n\u001b[1;32m    404\u001b[0m                   initial_center_lanelet_ids\u001b[39m=\u001b[39;49minitial_center_lanelet_ids,\n\u001b[1;32m    405\u001b[0m                   initial_shape_lanelet_ids\u001b[39m=\u001b[39;49minitial_shape_lanelet_ids,\n\u001b[1;32m    406\u001b[0m                   initial_signal_state\u001b[39m=\u001b[39;49minitial_signal_state, signal_series\u001b[39m=\u001b[39;49msignal_series)\n\u001b[1;32m    407\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mprediction: Prediction \u001b[39m=\u001b[39m prediction\n\u001b[1;32m    408\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39minitial_meta_information_state \u001b[39m=\u001b[39m initial_meta_information_state\n",
      "File \u001b[0;32m~/Programs/ad_benchmark_learn/.venv/lib/python3.10/site-packages/commonroad/scenario/obstacle.py:80\u001b[0m, in \u001b[0;36mObstacle.__init__\u001b[0;34m(self, obstacle_id, obstacle_role, obstacle_type, obstacle_shape, initial_state, initial_center_lanelet_ids, initial_shape_lanelet_ids, initial_signal_state, signal_series)\u001b[0m\n\u001b[1;32m     78\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mobstacle_type: ObstacleType \u001b[39m=\u001b[39m obstacle_type\n\u001b[1;32m     79\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mobstacle_shape: Shape \u001b[39m=\u001b[39m obstacle_shape\n\u001b[0;32m---> 80\u001b[0m \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49minitial_state: InitialState \u001b[39m=\u001b[39m initial_state\n\u001b[1;32m     81\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39minitial_center_lanelet_ids: Optional[Set[\u001b[39mint\u001b[39m]] \u001b[39m=\u001b[39m initial_center_lanelet_ids\n\u001b[1;32m     82\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39minitial_shape_lanelet_ids: Optional[Set[\u001b[39mint\u001b[39m]] \u001b[39m=\u001b[39m initial_shape_lanelet_ids\n",
      "File \u001b[0;32m~/Programs/ad_benchmark_learn/.venv/lib/python3.10/site-packages/commonroad/scenario/obstacle.py:183\u001b[0m, in \u001b[0;36mObstacle.initial_state\u001b[0;34m(self, initial_state)\u001b[0m\n\u001b[1;32m    181\u001b[0m \u001b[39m@initial_state\u001b[39m\u001b[39m.\u001b[39msetter\n\u001b[1;32m    182\u001b[0m \u001b[39mdef\u001b[39;00m \u001b[39minitial_state\u001b[39m(\u001b[39mself\u001b[39m, initial_state: InitialState):\n\u001b[0;32m--> 183\u001b[0m     \u001b[39massert\u001b[39;00m \u001b[39misinstance\u001b[39m(initial_state, InitialState), (\n\u001b[1;32m    184\u001b[0m             \u001b[39m'\u001b[39m\u001b[39m<Obstacle/initial_state>: argument initial_state of wrong type. \u001b[39m\u001b[39m'\u001b[39m\n\u001b[1;32m    185\u001b[0m             \u001b[39m'\u001b[39m\u001b[39mExpected types: \u001b[39m\u001b[39m%s\u001b[39;00m\u001b[39m. Got type: \u001b[39m\u001b[39m%s\u001b[39;00m\u001b[39m.\u001b[39m\u001b[39m'\u001b[39m \u001b[39m%\u001b[39m (InitialState, \u001b[39mtype\u001b[39m(initial_state)))\n\u001b[1;32m    186\u001b[0m     \u001b[39mself\u001b[39m\u001b[39m.\u001b[39m_initial_state \u001b[39m=\u001b[39m initial_state\n\u001b[1;32m    187\u001b[0m     \u001b[39mself\u001b[39m\u001b[39m.\u001b[39m_initial_occupancy_shape \u001b[39m=\u001b[39m occupancy_shape_from_state(\u001b[39mself\u001b[39m\u001b[39m.\u001b[39m_obstacle_shape, initial_state)\n",
      "\u001b[0;31mAssertionError\u001b[0m: <Obstacle/initial_state>: argument initial_state of wrong type. Expected types: <class 'commonroad.scenario.state.InitialState'>. Got type: <class 'commonroad.scenario.state.CustomState'>."
     ]
    }
   ],
   "source": [
    "# run simulation, a video animation of the simulation is stored in the end\n",
    "scenario_without_ego, pps = simulate_without_ego(interactive_scenario_path=path_scenario,\n",
    "                                                 output_folder_path=path_video,\n",
    "                                                 create_video=True)\n",
    "# write simulated scenario to CommonRoad xml file\n",
    "fw = CommonRoadFileWriter(scenario_without_ego, pps, author, affiliation, source, tags)\n",
    "fw.write_to_file(os.path.join(path_scenarios_simulated, name_scenario + \"_no_ego.xml\"), OverwriteExistingFile.ALWAYS)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Simulation with a Plugged-in Motion Planner\n",
    "\n",
    "Naturally, we would like to test the performance of our motion planners against the interactive scenarios. This is possible thanks to the [CR-SUMO Interface](https://gitlab.lrz.de/tum-cps/commonroad-sumo-interface), which provides an easy-to-use interface for retrieving and setting the states of the ego vehicles during SUMO simulation.\n",
    "\n",
    "The simulation is executed by calling the function **simulate_with_planner()**. It has the same arguments as **simulate_without_ego()**, with one extra optional argument:\n",
    "* **create_ego_obstacle**: indicates whether to create obstacles from the planned trajectories as the ego vehicles.\n",
    "\n",
    "The motion planner is plugged in at `simulation/simulations.py/simulate_scenario()`. A demo planner is given in lines 134-146, which simply decelerates the ego vehicle to full stop:\n",
    "```python\n",
    "next_state = copy.deepcopy(state_current_ego)\n",
    "# ====== plug in your motion planner here\n",
    "# example motion planner which decelerates to full stop\n",
    "a = -5.0\n",
    "dt = 0.1\n",
    "if next_state.velocity > 0:\n",
    "    v = next_state.velocity\n",
    "    x, y = next_state.position\n",
    "    o = next_state.orientation\n",
    "\n",
    "    next_state.position = np.array([x + v * cos(o) * dt, y + v * sin(o) * dt])\n",
    "    next_state.velocity += a * dt\n",
    "# ====== end of motion planner\n",
    "```\n",
    "\n",
    "We save the simulated scenario to a local CommonRoad xml file. Optionally, we can also save the planned trajectory to a solution file."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 49,
   "metadata": {
    "pycharm": {
     "is_executing": true
    }
   },
   "outputs": [],
   "source": [
    "# run simulation, an animation of the simulation is stored in the end\n",
    "scenario_with_planner, pps, ego_vehicles_planner = simulate_with_planner(interactive_scenario_path=path_scenario,\n",
    "                                                                         output_folder_path=path_video,\n",
    "                                                                         create_video=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 50,
   "metadata": {
    "pycharm": {
     "is_executing": true,
     "name": "#%%\n"
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Replace file /home/edmond/Softwares/commonroad/commonroad-interactive-scenarios-tumcps/tutorials/../outputs/simulated_scenarios/DEU_Cologne-63_5_I-1_planner.xml\n",
      "Trajectory saved to solution file.\n"
     ]
    }
   ],
   "source": [
    "# write simulated scenario to CommonRoad xml file\n",
    "if scenario_with_planner:\n",
    "    # write simulated scenario to file\n",
    "    fw = CommonRoadFileWriter(scenario_with_planner, pps, author, affiliation, source, tags)\n",
    "    fw.write_to_file(os.path.join(path_scenarios_simulated, name_scenario + \"_planner.xml\"), OverwriteExistingFile.ALWAYS)\n",
    "    \n",
    "    # save the planned trajectory to solution file\n",
    "    save_solution(scenario_with_planner, pps, ego_vehicles_planner, vehicle_type, vehicle_model, cost_function,\n",
    "                  path_solutions, overwrite=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3. Simulation with a Solution Trajectory\n",
    "\n",
    "Provided that we have a solution trajectory to the interactive scenario, we can easily replay and re-simulate the scenario.\n",
    "\n",
    "The simulation is executed by calling the function **simulate_with_solution()**. It has the same arguments as **simulate_without_ego()**, with one extra mandatory argument:\n",
    "* **solution**: solution file to the planning problem.\n",
    "\n",
    "Here, we re-simulate the scenario with a pre-computed solution trajectory."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 53,
   "metadata": {
    "pycharm": {
     "is_executing": true
    }
   },
   "outputs": [],
   "source": [
    "name_solution = \"solution_KS1:TR1:DEU_Cologne-63_5_I-1:2020a\"\n",
    "solution = CommonRoadSolutionReader.open(os.path.join(path_solutions, name_solution + \".xml\"))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 54,
   "metadata": {
    "pycharm": {
     "is_executing": true
    }
   },
   "outputs": [],
   "source": [
    "# run simulation, a video of the simulation is stored in the end\n",
    "scenario_with_solution, pps, ego_vehicles_solution = simulate_with_solution(interactive_scenario_path=path_scenario,\n",
    "                                                                            output_folder_path=path_video,\n",
    "                                                                            solution=solution,\n",
    "                                                                            create_video=True)\n",
    "# write simulated scenario to CommonRoad xml file\n",
    "if scenario_with_solution:\n",
    "    # write simulated scenario to file\n",
    "    fw = CommonRoadFileWriter(scenario_with_solution, pps, author, affiliation, source, tags)\n",
    "    fw.write_to_file(os.path.join(path_scenarios_simulated, name_scenario + \"_solution.xml\"), OverwriteExistingFile.ALWAYS)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 4. Examining the Simulation Results\n",
    "\n",
    "Now, it's time to examine and compare the simulation results. A helper function **visualize_scenario_with_trajectory()** is called to animate the scenario. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 55,
   "metadata": {
    "pycharm": {
     "is_executing": true
    }
   },
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "%matplotlib inline\n",
    "visualize_scenario_with_trajectory(scenario_without_ego, pps)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 56,
   "metadata": {
    "pycharm": {
     "is_executing": true
    }
   },
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "visualize_scenario_with_trajectory(scenario_with_planner, pps, ego_vehicles_planner)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 57,
   "metadata": {
    "pycharm": {
     "is_executing": true
    }
   },
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "visualize_scenario_with_trajectory(scenario_with_solution, pps, ego_vehicles_solution)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As can be seen from the animation, when the ego vehicle (green rectangle) is present, the other traffic participants behave differently and react accordingly. To better compare the scenarios, you can find the created video files under `/outputs/videos/`, and saved scenarios under `/outputs/simulated_scenarios/`."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
